import math
from ctypes import *
import time
from InitialPositionOfGuide import *
import os

current_dir = os.path.dirname(os.path.abspath(__file__))


def check_if_zero(func):
    def wrapper(self, *args, **kwargs):
        code = self.ur_arm.StartRTClient()
        if code == 1:
            return func(self, *args, **kwargs)
        else:
            return 'Failed to open robotic arm connection'

    return wrapper


class RobotArmAuto:
    def __init__(self):
        self.ur_arm = cdll.LoadLibrary(current_dir + r'/RobotarmDriver.dll')
        self.ip_jxb = create_string_buffer(b'169.254.232.188')
        self.ur_arm.RTClient(self.ip_jxb)

    # Set digital output parameters
    def _digital_input_mes(self):
        self.d1 = c_int(0)
        self.d2 = c_int(0)
        self.d3 = c_int(0)
        self.d4 = c_int(0)
        self.d5 = c_int(0)
        self.d6 = c_int(0)
        self.d7 = c_int(0)
        self.d8 = c_int(0)
        return (byref(self.d1), byref(self.d2), byref(self.d3), byref(self.d4), byref(self.d5), byref(self.d6), byref(
            self.d7), byref(self.d8))

    # Gets configurable input information
    def _configurable_input_mes(self):
        self.c1 = c_int(0)
        self.c2 = c_int(0)
        self.c3 = c_int(0)
        self.c4 = c_int(0)
        self.c5 = c_int(0)
        self.c6 = c_int(0)
        self.c7 = c_int(0)
        self.c8 = c_int(0)
        return (byref(self.c1), byref(self.c2), byref(self.c3), byref(self.c4), byref(self.c5), byref(self.c6), byref(
            self.c7), byref(self.c8))

    # Current position parameters of the manipulator
    def _joint_position(self):
        self.base = c_float(0)
        self.shoulder = c_float(0)
        self.elbow = c_float(0)
        self.wrist1 = c_float(0)
        self.wrist2 = c_float(0)
        self.wrist3 = c_float(0)
        return (
            byref(self.base), byref(self.shoulder), byref(self.elbow), byref(self.wrist1), byref(self.wrist2), byref(
                self.wrist3))

    # Current tcp position parameters of the robot arm
    def _tool_position(self):
        self.x = c_float(0)
        self.y = c_float(0)
        self.z = c_float(0)
        self.rx = c_float(0)
        self.ry = c_float(0)
        self.rz = c_float(0)
        return (
            byref(self.x), byref(self.y), byref(self.z), byref(self.rx), byref(self.ry), byref(
                self.rz))

    # Get the current position of the robot arm
    @check_if_zero
    def Get_joint_position(self):
        self.ur_arm.Get_joint_position(*self._joint_position())
        jxb_wz = {'base': self.base.value, 'shoulder': self.shoulder.value, 'elbow': self.elbow.value,
                  'wrist1': self.wrist1.value, 'wrist2': self.wrist2.value, 'wrist3': self.wrist3.value}
        jxb_wz1 = [ self.base.value, self.shoulder.value,  self.elbow.value,
                  self.wrist1.value,  self.wrist2.value, self.wrist3.value]
        return jxb_wz,jxb_wz1

    # Obtain the TCP position of the robot arm
    @check_if_zero
    def Get_tool_position(self):
        self.ur_arm.Get_tool_position(*self._tool_position())
        jxb_wz = {'x': self.x.value, 'y': self.y.value, 'z': self.z.value,
                  'rx': self.rx.value, 'ry': self.ry.value, 'rz': self.rz.value}
        return jxb_wz

    @check_if_zero
    def adjust_rotation(self, rx, ry, rz):
        # test2 = cdll.LoadLibrary(r'C:\Users\123456\Desktop\LIMS\web\RobotarmDriver.dll')
        if math.fabs(rx) <= 0.6 and math.fabs(ry) <= 0.6 and math.fabs(rz) <= 0.6:
            print('Adjust rotation done...')
            return 1
        else:
            print('调整角度')
            print(rx, ry, rz)

            print('移动rx')

            self.ur_arm.Set_tcp_RX_rotation(c_float(-rz))  # 绕Y旋转

            self.ur_arm.Set_tcp_RY_rotation(c_float(rx))
            # # #
            self.ur_arm.Set_tcp_RZ_rotation(c_float(-ry))

            while self.ur_arm.IsRobotMoving() == 2:
                time.sleep(1)

            return 'success'

    @check_if_zero
    def adjust_rotation_Z(self, Z):
        # test2 = cdll.LoadLibrary(r'C:\Users\123456\Desktop\LIMS\web\RobotarmDriver.dll')

        x = c_float(0)
        y = c_float(0)
        z = c_float(0)
        rx = c_float(0)
        ry = c_float(0)
        rz = c_float(0)
        self.ur_arm.Get_tool_position(byref(x), byref(y), byref(z), byref(rx), byref(ry), byref(rz))
        print('调整方向，距离')
        print(z.value + Z)
        if z.value + Z > 0.32:
            code = self.ur_arm.Set_tool_move(x, y, c_float(0.32), rx, ry, rz, c_float(0.1), c_float(0.1))
        elif z.value + Z < 0.106:
            print(0.106)
            code = self.ur_arm.Set_tool_move(x, y, c_float(0.106), rx, ry, rz, c_float(0.1), c_float(0.1))
        else:
            code = self.ur_arm.Set_tool_move(x, y, c_float(z.value + Z), rx, ry, rz, c_float(0.1), c_float(0.1))
        if code == 0:
            return 'Move failure'
        self.ur_arm.IsRobotMoving.restype = c_double
        while self.ur_arm.IsRobotMoving() == 2:
            time.sleep(1)
        # if abs(a) > 2: test2.Set_tool_move(c_float(x.value), c_float(y.value - a/1000), c_float(z.value),
        # c_float(rx.value), c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1)) else:
        # test2.Set_tool_move(c_float(x.value), c_float(y.value), c_float(z.value + b/1000), c_float(rx.value),
        # c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1))
        return 0
    
    @check_if_zero
    def adjust_rotation_Z_no_limit(self, Z):
        x = c_float(0)
        y = c_float(0)
        z = c_float(0)
        rx = c_float(0)
        ry = c_float(0)
        rz = c_float(0)
        self.ur_arm.Get_tool_position(byref(x), byref(y), byref(z), byref(rx), byref(ry), byref(rz))
        if y.value < -0.499:
            raise ''
        code = self.ur_arm.Set_tool_move(x, y, c_float(z.value + Z), rx, ry, rz, c_float(0.1), c_float(0.1))
        if code == 0:
            return 'Move failure'
        self.ur_arm.IsRobotMoving.restype = c_double
        while self.ur_arm.IsRobotMoving() == 2:
            time.sleep(1)
        return 0
    
    @check_if_zero
    def adjust_Y(self, Y):
        # test2 = cdll.LoadLibrary(r'C:\Users\123456\Desktop\LIMS\web\RobotarmDriver.dll')

        x = c_float(0)
        y = c_float(0)
        z = c_float(0)
        rx = c_float(0)
        ry = c_float(0)
        rz = c_float(0)
        self.ur_arm.Get_tool_position(byref(x), byref(y), byref(z), byref(rx), byref(ry), byref(rz))
        print('调整方向，距离')
        print(y.value + Y)
        code = self.ur_arm.Set_tool_move(x, c_float(y.value + Y), z, rx, ry, rz, c_float(0.1), c_float(0.1))
        if code == 0:
            return 'Move failure'
        self.ur_arm.IsRobotMoving.restype = c_double
        while self.ur_arm.IsRobotMoving() == 2:
            time.sleep(1)
        # if abs(a) > 2: test2.Set_tool_move(c_float(x.value), c_float(y.value - a/1000), c_float(z.value),
        # c_float(rx.value), c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1)) else:
        # test2.Set_tool_move(c_float(x.value), c_float(y.value), c_float(z.value + b/1000), c_float(rx.value),
        # c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1))
        return 0

    @check_if_zero
    def adjust_rotation_X(self, X):
        # test2 = cdll.LoadLibrary(r'C:\Users\123456\Desktop\LIMS\web\RobotarmDriver.dll')

        x = c_float(0)
        y = c_float(0)
        z = c_float(0)
        rx = c_float(0)
        ry = c_float(0)
        rz = c_float(0)
        self.ur_arm.Get_tool_position(byref(x), byref(y), byref(z), byref(rx), byref(ry), byref(rz))
        print('调整方向，距离')
        print(x.value + X)
        code = self.ur_arm.Set_tool_move(c_float(x.value + X), y, z, rx, ry, rz, c_float(0.1), c_float(0.1))
        if code == 0:
            return 'Move failure'
        self.ur_arm.IsRobotMoving.restype = c_double
        while self.ur_arm.IsRobotMoving() == 2:
            time.sleep(1)
        # if abs(a) > 2: test2.Set_tool_move(c_float(x.value), c_float(y.value - a/1000), c_float(z.value),
        # c_float(rx.value), c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1)) else:
        # test2.Set_tool_move(c_float(x.value), c_float(y.value), c_float(z.value + b/1000), c_float(rx.value),
        # c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1))
        return 0

    @check_if_zero
    def adjust_translation(self, a, b):
        # test2 = cdll.LoadLibrary(r'C:\Users\123456\Desktop\LIMS\web\RobotarmDriver.dll')

        x = c_float(0)
        y = c_float(0)
        z = c_float(0)
        rx = c_float(0)
        ry = c_float(0)
        rz = c_float(0)
        self.ur_arm.Get_tool_position(byref(x), byref(y), byref(z), byref(rx), byref(ry), byref(rz))
        if abs(a) <= 2 and abs(b) <= 2:

            return 1
        else:
            print('调整方向，距离')
            if z.value + b / 1000 > 0.106:
                self.ur_arm.Set_tool_move(c_float(x.value + a / 1000), c_float(y.value), c_float(z.value + b / 1000),
                                      c_float(rx.value), c_float(ry.value), c_float(rz.value), c_float(0.1),
                                      c_float(0.1))
            else:
                self.ur_arm.Set_tool_move(c_float(x.value + a / 1000), c_float(y.value), c_float(0.106),
                                          c_float(rx.value), c_float(ry.value), c_float(rz.value), c_float(0.1),
                                          c_float(0.1))
            while self.ur_arm.IsRobotMoving() == 2:
                time.sleep(1)
            # if abs(a) > 2: test2.Set_tool_move(c_float(x.value), c_float(y.value - a/1000), c_float(z.value),
            # c_float(rx.value), c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1)) else:
            # test2.Set_tool_move(c_float(x.value), c_float(y.value), c_float(z.value + b/1000), c_float(rx.value),
            # c_float(ry.value), c_float(rz.value), c_float(0.1), c_float(0.1))
            return 0

    @check_if_zero
    def adjust_translation_request(self, b):
        print(b)
        # test2 = cdll.LoadLibrary(r'C:\Users\123456\Desktop\LIMS\web\RobotarmDriver.dll')

        x = c_float(0)
        y = c_float(0)
        z = c_float(0)
        rx = c_float(0)
        ry = c_float(0)
        rz = c_float(0)
        self.ur_arm.Get_tool_position(byref(x), byref(y), byref(z), byref(rx), byref(ry), byref(rz))
        print(z.value + (b[0] / 1000))
        if z.value + (b[0] / 1000) > 0.106:
            # if isinstance(z.value + (b[0]/1000) > -0.016, np.bool_):
            return 'yes'
        return 'no'

    # Manipulator movement
    @check_if_zero
    def Set_joint_move(self, position, accelerated_speed, speed):
        base = c_float(float(position.base))
        shoulder = c_float(float(position.shoulder))
        elbow = c_float(float(position.elbow))
        wrist1 = c_float(float(position.wrist1))
        wrist2 = c_float(float(position.wrist2))
        wrist3 = c_float(float(position.wrist3))
        accelerated_speed = c_float(float(accelerated_speed))
        speed = c_float(float(speed))
        code = self.ur_arm.Set_joint_move(base, shoulder, elbow, wrist1, wrist2, wrist3, accelerated_speed, speed)
        if code == 0:
            return 'Move failure'
        self.ur_arm.IsRobotMoving.restype = c_double
        while self.ur_arm.IsRobotMoving() == 2:
            time.sleep(1)
        return 'Move complete'

    # Robot arm moves TCP
    @check_if_zero
    def Set_tool_move(self, position, accelerated_speed, speed, up_down):
        if up_down == 0:
            z = c_float(float(position.z))
        else:
            z = c_float(float(position.z) + 0.2)
        x = c_float(float(position.x))
        y = c_float(float(position.y))
        rx = c_float(float(position.rx))
        ry = c_float(float(position.ry))
        rz = c_float(float(position.rz))
        accelerated_speed = c_float(float(accelerated_speed))
        speed = c_float(float(speed))
        code = self.ur_arm.Set_tool_move(x, y, z, rx, ry, rz, accelerated_speed, speed)
        if code == 0:
            return 'Move failure'
        self.ur_arm.IsRobotMoving.restype = c_double
        while self.ur_arm.IsRobotMoving() == 2:
            time.sleep(1)
        return 'Move complete'

    # The robotic arm stops moving
    @check_if_zero
    def Stop_joint_move(self):
        self.ur_arm.Stop_joint_move()
        return 'The robotic arm stop function is complete'

    # The TCP of the mechanical arm stops moving
    @check_if_zero
    def Stop_tcp_move(self):
        self.ur_arm.Stop_tcp_move()
        return 'The TCP stop function of the robot arm is executed successfully'

    # Mechanical arm clamp
    @check_if_zero
    def Set_standard_digital_out_clamp_arm(self, loction):
        arm.Set_standard_digital_out(0, 0)  # 夹取手机仓
        time.sleep(0.1)
        # time.sleep(0.1)
        if loction == 1 or loction == 2:
            self.ur_arm.Set_standard_digital_out(4, 0)  # 松开手机定位夹
        if loction == 4 or loction == 3:
            self.ur_arm.Set_standard_digital_out(5, 0)  # 松开手机定位夹
        if loction == 6 or loction == 5:
            self.ur_arm.Set_standard_digital_out(6, 0)  # 松开手机定位夹
        # self.ur_arm.Set_standard_digital_out(0, 0)
        # self.ur_arm.Get_digitalinput_mes(*self._digital_input_mes())
        # if self.d1.value == 0:
        return 'Successful clamp'

    # Arm release
    @check_if_zero
    def Set_standard_digital_out_release_arm(self):
        self.ur_arm.Set_standard_digital_out(0, 1)

        self.ur_arm.Get_digitalinput_mes(*self._digital_input_mes())  # 修改
        # if self.d1.value == 1:
        return 'Release success'

    @check_if_zero
    def Set_standard_digital_out(self, id, state):
        self.ur_arm.Set_standard_digital_out(id, state)

        self.ur_arm.Get_digitalinput_mes(*self._digital_input_mes())  # 修改
        # if self.d1.value == 1:
        return 'Release success'

    # 860 clamp
    @check_if_zero
    def Set_standard_digital_out_clamp_860(self):
        self.ur_arm.Set_standard_digital_out(3, 0)
        self.ur_arm.Get_digitalinput_mes(*self._digital_input_mes())
        # if self.d3.value == 0:
        return 'Successful clamp'
        return 'Clamp failure'

    # 860 Release
    @check_if_zero
    def Set_standard_digital_out_release_860(self):
        self.ur_arm.Set_standard_digital_out(3, 1)
        self.ur_arm.Get_digitalinput_mes(*self._digital_input_mes())
        # if self.d3.value == 1:
        return 'Release success'
        return 'Release failure'

    # @check_if_zero
    def Get_configurable_input_mes(self):
        self.ur_arm = cdll.LoadLibrary(current_dir + r'/RobotarmDriver.dll')
        self.ip_jxb = create_string_buffer(b'169.254.232.188')
        self.ur_arm.RTClient(self.ip_jxb)
        code = self.ur_arm.StartRTClient()
        self.c1 = c_int(1)
        self.c2 = c_int(1)
        self.c3 = c_int(10)
        self.c4 = c_int(0)
        self.c5 = c_int(0)
        self.c6 = c_int(0)
        self.c7 = c_int(0)
        self.c8 = c_int(0)
        self.ur_arm.Get_digitalinput_mes(byref(self.c1), byref(self.c2), byref(self.c3), byref(self.c4), byref(self.c5), byref(self.c6), byref(
            self.c7), byref(self.c8))
        # self.ur_arm.Get_digitalinput_mes(*self._configurable_input_mes())
        return [self.c1.value, self.c2.value, self.c3.value, self.c4.value, self.c5.value, self.c6.value,self.c7.value,self.c8.value]


arm = RobotArmAuto()
if __name__ == '__main__':
    # print(arm.Get_configurable_input_mes())
    # arm.adjust_rotation_X(-0.064)
    print(arm.Get_joint_position())
    # print(arm.adjust_Y(-0.1))
    # print(arm.adjust_rotation_ZZ(-0.1))

    print(arm.Get_tool_position())
    # arm.adjust_rotation_Z(-0.200)
    # arm.adjust_rotation_Z(0.0562)
    # arm.Set_standard_digital_out(6, 1)
    # print(arm.Get_configurable_input_mes())
    # arm.Set_joint_move(Alignment_Position, 0.1, 0.1)
    #     arm.Set_joint_move(Clamp_position_fix, 0.5, 0.5)
    # arm.Set_tool_move(Iphone_position_one, 0.1, 0.1, 1)
#     res = arm.Set_standard_digital_out_release_arm()
# if res == '释放成功':
#     arm.Set_tool_move(Iphone_position_fix, 0.1, 0.1, 0)
#     res = arm.Set_standard_digital_out_clamp_arm()
#     if res == '夹取成功':
#         arm.Set_tool_move(Iphone_position_fix, 0.1, 0.1, 1)
#         arm.Set_joint_move(Alignment_Position, 0.5, 0.5)
